package jp.seraph.robocup.soccer.sample.agent;

import java.io.BufferedWriter;

import jp.seraph.jsade.core.Agent;
import jp.seraph.jsade.core.Player;
import jp.seraph.jsade.core.World;
import jp.seraph.jsade.effector.NaoEffectorKind;
import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.model.nao.NaoJointIdentifier;

public class SampleAgent implements Agent {
    public SampleAgent(BufferedWriter aWriter) {
        System.out.println("start");
        mWriter = aWriter;
    }

    private boolean mFlag = true;

    private double mLast = 0;

    private double mLastValue = 0;

    private int mCount = 0;

    private BufferedWriter mWriter;

    public void think(World aWorld, Player aPlayer) {
        Player tPlayer = aPlayer;

//         if (mFlag) {
//            if (tPlayer.setJointAngle(NaoJointIdentifier.HJ1, Angle.createAngleAsDegree(100)).equals(AngleVelocity.ZERO))
//                mFlag = false;
//        } else {
//            if (tPlayer.setJointAngle(NaoJointIdentifier.HJ1, Angle.createAngleAsDegree(-100)).equals(AngleVelocity.ZERO))
//                mFlag = true;
//        }

        mCount++;
        double tValue = ((double) (mCount / 100)) / 10;
        if (mCount > 8000)
            System.exit(0);

        if (mFlag) {
            if (tPlayer.getJointAngle(NaoJointIdentifier.HJ1).getDegree() > 100)
                mFlag = false;
            else
                tPlayer.append(NaoEffectorKind.HE1, tValue);
        } else {
            if (tPlayer.getJointAngle(NaoJointIdentifier.HJ1).getDegree() < -100)
                mFlag = true;
            else
                tPlayer.append(NaoEffectorKind.HE1, -tValue);

        }
        double tCurrent = tPlayer.getJointAngle(NaoJointIdentifier.HJ1).getDegree();
        double tDiff = Math.abs(tCurrent - mLast);

        try {
            mWriter.write(mLastValue + "," + tDiff);
            mWriter.newLine();
        } catch (Exception e) {
            e.printStackTrace();
        }

        // System.out.println(tCurrent + ":" + (tCurrent - mLast));
        mLast = tCurrent;
        mLastValue = tValue;

        // SeeObject ball = aWorld.getSeeObject(SeeObjectType.BALL);

        // System.out.println(tPlayer.getJointAngle(NaoJointIdentifier.HJ1).getDegree());

    }

    private void set(Player aPlayer, String aJoint, double aAngle) {
        aPlayer.setJointAngle(NaoJointIdentifier.getJointIdentifier(aJoint), Angle.createAngleAsDegree(aAngle));
    }
}
